#include "Pathfind.h"
#include "../GlobalData/GlobalData.h"
#include <iostream> // for debugging purposes only

PathFinder::PathFinder()
{
}

PathFinder::PathFinder(int x, int y, float MaxSearch) : Done(false), X(x + 7), Y(y + 7), 
														SearchDistance(MaxSearch), 
														ClosedList(NULL), 
														OpenList(NULL)
{
}

PathFinder::~PathFinder()
{
}

std::vector<ObjectFound> *PathFinder::GetObjectsList()
{
	return &ObjectsFound;
}

bool PathFinder::NodeValid(int x, int y)
{
	return true;
}

void PathFinder::GenerateRandomPath()
{
	int dx = rand() % 3 - 1; // Ranged from -1 to 1
	int dy = rand() % 3 - 1; // Ranged from -1 to 1

	while ( ( ((X + dx) < 0) || ((Y + dy) < 0) || ((Y + dy) >= GRID_HEIGHT) || ((X + dx) >= GRID_WIDTH) ) ||
		(dx == 0 && dy == 0) || // While looking at the node we're at
		(gWorldMap.WorldGrid[Y + dy][X + dx].IsWall) || // Or looking at a node that's a wall
		( (dx != 0 && dy != 0) && ( gWorldMap.WorldGrid[Y + dy][X].IsWall || gWorldMap.WorldGrid[Y][X + dx].IsWall) ) ) // Or walking diagonal across a wall
	{ // get some new random numbers
		dx = rand() % 3 - 1;
		dy = rand() % 3 - 1;
	}

	// Kind of pedantic, but may end up being necessary
	Grid[Y + dy][X + dx].Parent = &Grid[Y][X];

	// Record the randomly generated path
	RecordPath(X + dx,Y + dy);
	return;
}

void PathFinder::ResetPathFinder(int x, int y, float MaxSearch)
{
	X = x;
	Y = y;
	SearchDistance = MaxSearch;
  ObjectsFound.clear();
  this->Done = false;
  this->OpenList = NULL;
  this->ClosedList = NULL;
	return;
}

int PathFinder::Search(void)
{
	int ProgressTracker;

  ResetPathFinder( X, Y, SearchDistance );

	for (int y = 0;y < GRID_HEIGHT; ++y)
	{
		for (int x = 0;x < GRID_WIDTH; ++x)
		{
			Grid[y][x].NodeStatus = Unused;
			Grid[y][x].Distance = VERY_BIG_FLOAT;
			Grid[y][x].NextClosed = NULL;
			Grid[y][x].NextOpen = NULL;
			Grid[y][x].Parent = NULL;
			Grid[y][x].X = (float)x;
			Grid[y][x].Y = (float)y;
		}
	}

	Grid[Y][X].Distance = 0;
	PutOnOpenList(X,Y);

	ProgressTracker = RecursiveSearch();

  GenerateRandomPath();

	return ProgressTracker;
}

void PathFinder::PutOnClosedList(int x, int y)
{
	// Set some intermediate values
	NodeData *Tracker = ClosedList;
	NodeData *TrackerShadow = NULL;

	// Handle the case in which this is the first node on the closed list (ClosedList == NULL)
	if (ClosedList == NULL)
	{
		ClosedList = &Grid[y][x];
		return;
	}

	// Handle the case in which the Closed head is being replaced (TrackerShadow == NULL)
	if (Grid[y][x].Distance < Tracker->Distance)
	{
		Grid[y][x].NextClosed = Tracker;
		ClosedList = &Grid[y][x];
		return;
	}

	TrackerShadow = Tracker;
	Tracker = Tracker->NextClosed;

	// Handle most other case (ClosedList != NULL) and (TrackerShadow != NULL)
	while (Tracker != NULL)
	{
		if (Grid[y][x].Distance < Tracker->Distance)
		{
			TrackerShadow->NextClosed = &Grid[y][x];
			Grid[y][x].NextClosed = Tracker;
			return;
		}

		TrackerShadow = Tracker;
		Tracker = Tracker->NextClosed;
	}

	// Handle the final case (Tracker == NULL) and (TrackerShadow != NULL), this happens when pushing onto the end of hte list
	TrackerShadow->NextClosed = &Grid[y][x];
	return;
}

void PathFinder::PutOnOpenList(int x, int y)
{
	// Set some intermediate values
	NodeData *Tracker = OpenList;
	NodeData *TrackerShadow = NULL;

	// Handle the case in which this is the first node on the closed list (OpenList == NULL)
	if (OpenList == NULL)
	{
		OpenList = &Grid[y][x];
		return;
	}

	// Handle the case in which the Closed head is being replaced (TrackerShadow == NULL)
	if (Grid[y][x].Distance < Tracker->Distance)
	{
		Grid[y][x].NextOpen = Tracker;
		OpenList = &Grid[y][x];
		return;
	}

	TrackerShadow = Tracker;
	Tracker = Tracker->NextOpen;

	// Handle most other case (ClosedList != NULL) and (TrackerShadow != NULL)
	while (Tracker != NULL)
	{
		if (Grid[y][x].Distance < Tracker->Distance)
		{
			TrackerShadow->NextOpen = &Grid[y][x];
			Grid[y][x].NextOpen = Tracker;
			return;
		}

		TrackerShadow = Tracker;
		Tracker = Tracker->NextOpen;
	}

	// Handle the final case (Tracker == NULL) and (TrackerShadow != NULL), this happens when pushing onto the end of hte list
	TrackerShadow->NextOpen = &Grid[y][x];
	return;
}

void PathFinder::RecordPath(int x, int y)
{
	// Record a path from the coords passed in
	ObjectFound NewObject;
	NodeData *Tracker = &Grid[y][x];
	float TempHolder[2];

	NewObject.Distance = Grid[y][x].Distance;
  NewObject.objPtr = gWorldMap.WorldGrid[y][x].Object;
	ObjectsFound.push_back(NewObject);

	// While the tracker has not found the end of the list of parents, keep pushing and iterating
	while(Tracker != NULL)
	{
		TempHolder[0] = Tracker->X - 7;
		TempHolder[1] = Tracker->Y - 7;
		ObjectsFound.back().Path.push_back(Vector2f(TempHolder));

		Tracker = Tracker->Parent;
	}

	return;
}

bool PathFinder::GoodToConsider(int x, int y)
{
	return (x >= 0) &&
		(y >= 0) &&
		(x < GRID_WIDTH) &&
		(y < GRID_HEIGHT) &&
		(Grid[y][x].NodeStatus == Unused) &&
		(!gWorldMap.WorldGrid[y][x].IsWall);
}

int PathFinder::RecursiveSearch(void)
{
	if (OpenList == NULL)
	{ // If ther are no more nodes to consider, return -1, looked at all nodex
		return -1;
	}

	// Pop a node off the open list, this is the one we are considering
	NodeData *Consider = OpenList;
	OpenList = OpenList->NextOpen;

	if (Consider->Distance > this->SearchDistance)
	{ // -3 is the value for finding a node too far away
		return -3;
	}

	int ConsiderX = (int)Consider->X;
	int ConsiderY = (int)Consider->Y;

	// Put all nodes around this one on the open list, if they're unused
	for (int dx = -1;dx <= 1;++dx)
	{
		for (int dy = -1;dy <= 1;++dy)
		{
			if ( GoodToConsider( ConsiderX + dx, ConsiderY + dy) && !(dx == 0 && dy == 0) )
			{
				if ( ( ( dx != 0 && dy != 0 ) && // if looking at a diagonal path
					(!gWorldMap.WorldGrid[ConsiderY + dy][ConsiderX].IsWall) && // and there's a wall blocking the diagonal
					(!gWorldMap.WorldGrid[ConsiderY][ConsiderX + dx].IsWall) ) || // don't add the node to the open list
					(dx == 0 || dy == 0) ) // If this is a straight path, don't worry about walls
				{
					Grid[ConsiderY + dy][ConsiderX + dx].Parent = &Grid[ConsiderY][ConsiderX];
					Grid[ConsiderY + dy][ConsiderX + dx].NodeStatus = Open;
					if (dx == 0 || dy == 0)
					{
						Grid[ConsiderY + dy][ConsiderX + dx].Distance = Grid[ConsiderY][ConsiderX].Distance + 1.0f;
					}
					else
					{
						Grid[ConsiderY + dy][ConsiderX + dx].Distance = Grid[ConsiderY][ConsiderX].Distance + SQRT_2;
					}

					PutOnOpenList(ConsiderX + dx,ConsiderY + dy);
				}
			}
		}
	}

	// Record any data about the node currently being considered
	if (gWorldMap.WorldGrid[ConsiderY][ConsiderX].Object != NULL && (Grid[ConsiderY][ConsiderX].Distance != 0) )
	{
		RecordPath(ConsiderX,ConsiderY);
	}

	// Also, record a path at random if there is no path recorded yet, this ansures a bit of entropy in the "wander" behavior
	//if ( ((rand() % 100) <= 2) && (ObjectsFound.empty()) )
	//{
	//	RecordPath(ConsiderX,ConsiderY);
	//}

	// Mark the current node as closed
	PutOnClosedList(ConsiderX,ConsiderY);
	Grid[ConsiderY][ConsiderX].NodeStatus = Closed;

	//DataDump();

	return RecursiveSearch();
}

void PathFinder::DataDump()
{
	std::cout << "\n";
	std::cout << "\n";
	std::cout << "\n";

	for (int y = 0;y < GRID_HEIGHT; ++y)
	{
		for (int x = 0;x < GRID_WIDTH; ++x)
		{
			printf("%4.4f\t",Grid[y][x].Distance);
		}
		printf("\n");
	}

	std::cout << std::endl;

	for (int y = 0;y < GRID_HEIGHT;++y)
	{
		for (int x = 0;x < GRID_WIDTH;++x)
		{
			switch(Grid[y][x].NodeStatus)
			{
			case Closed:
				std::cout << "C ";
				break;
			case Open:
				std::cout << "O ";
				break;
			case Unused:
				std::cout << "U ";
				break;
			default:
				std::cout << "  ";
				break;
			};
		}
		std::cout << "\n";
	}

	std::cout << std::endl;

	return;
}
